#include "stdafx.h"

#include "Blob.h"
#include "Ray.h"

using namespace rt;

static const XMVECTOR EPS_VEC = XMVectorReplicate(0.0001f);
static const float EPS = 0.00001f;

const float SMALL_RAY_STEP = 0.03f;
const float BIG_RAY_STEP = 0.4f;
const float MAX_RAY_LENGTH = 100.0f;

const float BLOB_PREC = 0.03f;

const XMVECTOR SMALL_RAY_STEP_VEC = XMVectorReplicate(SMALL_RAY_STEP);
const XMVECTOR BIG_RAY_STEP_VEC = XMVectorReplicate(BIG_RAY_STEP);

Metaball * Blob::CreateMetaball()
{
	uint ball = BallsNum++;
	assert(ball < TAB_SIZE);
	return &Balls[ball];
}

void rt::IntersectBlobs_All_4RaysSingleOrigin(const Blob * blobs, int num, const Ray4_SingleOrigin & rays, XMVECTOR & intersIndex, XMVECTOR & depth)
{
	for (int i = 0; i < num ; i++)
	{
		XMVECTOR newDepth = XMVectorSplatInfinity();
		
		for (uint rayIdx = 0; rayIdx < 4 ; rayIdx++)
		{
			XMVECTOR rayPos = rays.Orig;
			XMVECTOR rayDir = rays.GetDir(rayIdx);
			XMVECTOR smallRayStep = XMVectorMultiply(rayDir, SMALL_RAY_STEP_VEC);
			XMVECTOR bigRayStep = XMVectorMultiply(rayDir, BIG_RAY_STEP_VEC);

			for (float t = 0.0f; t < MAX_RAY_LENGTH; t += BIG_RAY_STEP) // Check ray taking big steps.
			{
				rayPos = XMVectorAdd(rayPos, bigRayStep);

				if(blobs[i].isInside(rayPos)) // If jumped into field...
				{
					float tStart = t - BIG_RAY_STEP;
					float tEnd = t;

					float tPos = tEnd;

					float oldInside = 1.0f;
					bool forward = false;

					

					//while(true)
					//{
					//	float oldTPos = tPos;
					//	tPos = (tStart + tEnd) * 0.5f;
					//	float move = (tEnd - tStart) * 0.5f;//abs(oldTPos - tPos);

					//	rayPos = XMVectorMultiplyAdd(rayDir, XMVectorReplicate(tPos), rays.Orig);
					//	
					//	float newInside = blobs[i].fieldStrength(rayPos) - 1.0f;

					//	float mulSign = newInside * oldInside;

					//	if(newInside >= 0.0f && oldInside >= 0.0f)
					//		forward = false;
					//	else if(newInside < 0.0f && oldInside < 0.0f)
					//		forward = true;
					//	else
					//		forward = !forward;

					//	oldInside = newInside;

					//	if(move < BLOB_PREC || abs(newInside) < 0.05f)
					//	{
					//		newDepth = XMVectorSetByIndex(newDepth, tPos, rayIdx);
					//		goto stop;
					//	}

					//	if(forward)
					//	{
					//		tStart = tPos;
					//	}
					//	else
					//	{
					//		tEnd = tPos;
					//	}
					//}


					for(float t2 = t ; t2 > t-BIG_RAY_STEP ; t2 -= SMALL_RAY_STEP) // ...start from here and take small steps back...
					{
						rayPos = XMVectorSubtract(rayPos, smallRayStep);
						if(!blobs[i].isInside(rayPos)) // ...until again outside the field.
						{
							newDepth = XMVectorSetByIndex(newDepth, t2, rayIdx);
							goto stop;
						}
					}
				}
			}
stop:
			float stupidThingSoLabelCompiles;
		}


		// if newDepth < depth and ...
		UINT isNewDepthLessThanDepth;
		XMVECTOR isNewDepthLessThanDepthMask = XMVectorGreaterR(&isNewDepthLessThanDepth, depth, newDepth);
		if(XMComparisonAllFalse(isNewDepthLessThanDepth))
			continue;
		newDepth = XMVectorSelect(XMVectorZero(), newDepth, isNewDepthLessThanDepthMask); // mask out components that failed comparison,
		// so they automatically fail next one

		// ... and newDepth > EPSILON ...
		UINT isNewDepthGreaterThanEps;
		XMVECTOR isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);

		// ... set new depth(s) and intersected object indices ...
		depth = XMVectorSelect(depth, newDepth, isNewDepthGreaterThanEpsMask);
		intersIndex = XMVectorSelect(intersIndex, XMVectorSetInt(i, i, i, i), isNewDepthGreaterThanEpsMask);
	}
}

//void rt::IntersectSpheres_All_4Rays(const Sphere * spheres, int num, const Ray4 & rays, XMVECTOR & intersIndex, XMVECTOR & depth, XMVECTOR & intersType)
//{
//	for (int i = 0; i < num ; i++)
//	{
//		// B = dot(dist, rayDir)
//		XMVECTOR distX = XMVectorSubtract(rays.OrigX, XMVectorSplatX(spheres[i].CenterAndRadiusSquared));
//		XMVECTOR distY = XMVectorSubtract(rays.OrigY, XMVectorSplatY(spheres[i].CenterAndRadiusSquared));
//		XMVECTOR distZ = XMVectorSubtract(rays.OrigZ, XMVectorSplatZ(spheres[i].CenterAndRadiusSquared));
//
//		XMVECTOR mulX = XMVectorMultiply(distX, rays.DirX); // dist.x * rayDir.x
//		XMVECTOR mulY = XMVectorMultiply(distY, rays.DirY); // dist.y * rayDir.y
//		XMVECTOR B = XMVectorAdd(mulX, mulY); // dist.x * rayDir.x + dist.y * rayDir.y
//		XMVECTOR mulZ = XMVectorMultiply(distZ, rays.DirZ); // dist.z * rayDir.z
//		B = XMVectorAdd(B, mulZ); // dist.x * rayDir.x + dist.y * rayDir.y + dist.z * rayDir.z
//
//		// C = dot(dist, dist) - radius * radius
//		distX = XMVectorMultiply(distX, distX);
//		distY = XMVectorMultiply(distY, distY);
//		XMVECTOR distDotDist = XMVectorAdd(distX, distY);
//		distZ = XMVectorMultiply(distZ, distZ);
//		distDotDist = XMVectorAdd(distDotDist, distZ);
//
//		XMVECTOR C =  XMVectorSubtract(distDotDist, XMVectorSplatW(spheres[i].CenterAndRadiusSquared));
//
//		// delta = B*B - C
//		XMVECTOR delta = XMVectorSubtract(XMVectorMultiply(B, B), C);
//
//		UINT isDeltaNeg;
//		XMVECTOR isDeltaNegMask = XMVectorGreaterOrEqualR(&isDeltaNeg, XMVectorZero(), delta);
//
//		// if delta < 0.0f, move on
//		if(XMComparisonAllTrue(isDeltaNeg))
//			continue;
//
//		// root = -b - sqrtf(delta)
//		XMVECTOR root = XMVectorSubtract(XMVectorNegate(B), XMVectorSqrt(delta));
//		// set to meaningful values only components with non-negative delta
//		XMVECTOR newDepth = XMVectorSelect(root, XMVectorSplatInfinity(), isDeltaNegMask);
//		// same thing with secondary intersections (from inside the object to outside)
//		root = XMVectorAdd(XMVectorNegate(B), XMVectorSqrt(delta));
//		XMVECTOR outInters = XMVectorSelect(root, XMVectorSplatInfinity(), isDeltaNegMask);
//
//
//		//// if newDepth < depth and ...
//		//UINT isNewDepthLessThanDepth;
//		//XMVECTOR isNewDepthLessThanDepthMask = XMVectorGreaterOrEqualR(&isNewDepthLessThanDepth, depth, newDepth);
//		//if(XMComparisonAllFalse(isNewDepthLessThanDepth))
//		//	continue;
//		//newDepth = XMVectorSelect(XMVectorZero(), newDepth, isNewDepthLessThanDepthMask); // mask out components that failed comparison,
//		//// so they automatically fail next one
//
//		//// ... and newDepth > EPSILON ...
//		//UINT isNewDepthGreaterThanEps;
//		//XMVECTOR isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);
//
//		//// ... set new depth(s) and intersected object indices ...
//		//depth = XMVectorSelect(depth, newDepth, isNewDepthGreaterThanEpsMask);
//		//intersIndex = XMVectorSelect(intersIndex, XMVectorSetInt(i, i, i, i), isNewDepthGreaterThanEpsMask);
//		// 
//
//		// if newDepth > EPSILON ...
//		UINT isNewDepthGreaterThanEps;
//		XMVECTOR isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);
//
//		// take into account intersections from inside the box to the outside
//		newDepth = XMVectorSelect(outInters, newDepth, isNewDepthGreaterThanEpsMask);
//		XMVECTOR newIntersType = XMVectorSelect(XMVectorFalseInt(), XMVectorTrueInt(), isNewDepthGreaterThanEpsMask); // false value in intersType indicates that found intersection is from inside the box
//
//		isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);
//		newDepth = XMVectorSelect(XMVectorSplatInfinity(), newDepth, isNewDepthGreaterThanEpsMask);
//
//		// ..  and newDepth < depth
//		UINT isNewDepthLessThanDepth;
//		const XMVECTOR isNewDepthLessThanDepthMask = XMVectorGreaterOrEqualR(&isNewDepthLessThanDepth, depth, newDepth);
//		depth = XMVectorSelect(depth, newDepth, isNewDepthLessThanDepthMask);
//		intersIndex = XMVectorSelect(intersIndex, XMVectorSetInt(i, i, i, i), isNewDepthLessThanDepthMask);
//		intersType = XMVectorSelect(intersType, newIntersType, isNewDepthLessThanDepthMask);
//
//
//
//		//// ... and new pointer(s) to intersected object
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[0] != 0.0f) intersPtr[0] = (void*)&spheres[i];
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[1] != 0.0f) intersPtr[1] = (void*)&spheres[i];
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[2] != 0.0f) intersPtr[2] = (void*)&spheres[i];
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[3] != 0.0f) intersPtr[3] = (void*)&spheres[i];
//		// 
//	}
//}

void rt::GetBlobIntersectionData(const Blob & blob, XMVECTOR intersPoint, IntersectionData & data)
{
	data.Normal = XMVector3Normalize(XMVectorSubtract(intersPoint, blob.Balls[0].Center));

	XMVECTOR normalAccum = XMVectorZero();
	float fieldStr = 0.0f;
	for(uint i = 0; i < 4; i++)
	{
		XMVECTOR dist = XMVectorSubtract(intersPoint, blob.Balls[i].Center);
		float influence = blob.Balls[i].Radius / XMVectorGetX(XMVector3Dot(dist, dist));
		fieldStr += influence;
		dist = XMVector3Normalize(dist);
		normalAccum = XMVectorAdd(normalAccum, XMVectorMultiply(XMVectorReplicate(influence), dist));
	}

	data.Normal = XMVector3Normalize(XMVectorDivide(normalAccum, XMVectorReplicate(fieldStr)));
}


//
//IntersectionInfo MetaballGroup::GetIntersectionInfo(const Vec3 & point)
//{
//	float fieldStrength = 0.0f;
//	Vec3 normalAccum = Vec3(0,0,0);
//	Material matAccum;
//	matAccum.Zero();
//
//	//for (BallIter it = _balls.begin(); it != _balls.end(); it++)
//	//{
//	//	Vec3 dist = point - it->_center;
//
//	//	float influence = it->_radius / (dist.x * dist.x + dist.y * dist.y + dist.z * dist.z);
//	//	fieldStrength += influence;
//
//	//	dist.normalize();
//	//	normalAccum += dist * influence;
//
//	//	matAccum.MultiplyAndAdd(it->_material, influence);
//	//}
//	if(fieldStrength <= 0.0f)
//		return IntersectionInfo(Vec3(0,0,0), &_material);
//
//	matAccum /= fieldStrength;
//	_material = matAccum;
//
//	return IntersectionInfo(normalAccum / fieldStrength, &_material);
//}
